#include "realman_motor.h"
#include <string.h>

void realman_set_servo_position(uint8_t motor_id, float position)
{
    uint8_t cmd[4];
    *(int32_t*)cmd = (int32_t)(position * 10000);
    push_queue(0x200 | motor_id, cmd, 4);
}

void realman_set_position(uint8_t motor_id, float position)
{
    uint8_t cmd[6];
    int32_t pos;
    cmd[0] = 0x02;
    cmd[1] = 0x36;
    // *(int32_t*)&cmd[2] = (int32_t)(position * 10000);
    pos = position * 10000;
    memcpy(&cmd[2], &pos, 4);
    push_queue(motor_id, (uint8_t*)cmd, 6);
}

